﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using Microsoft.WindowsMobile.Samples.Location;

namespace Mirkuz.Mapping
{
    public class MapPoint
    {
        public double Latitude=0;
        public double Longitude=0;
        public int Altitude=0;

        public static MapPoint FromLocation(Mirkuz.LocationServices.Location location)
        {
            if (location.AltitudeValid)
                return new MapPoint(location.Latitude, location.Longitude, location.Altitude);
            
            return new MapPoint(location.Latitude, location.Longitude);
        }

        public static MapPoint FromGPSPosition(GpsPosition Position)
        {
            if (!Position.SeaLevelAltitudeValid
                 || !Position.LatitudeValid
                 || !Position.LongitudeValid                 
                 )
                throw new Exception("Invalid position");

            return new MapPoint(Position.Latitude, Position.Longitude,(int) Position.SeaLevelAltitude);
        }

        public MapPoint(double latitude, double longitude,int altitude)
        {
            Latitude = latitude;
            Longitude = longitude;
            Altitude = altitude;
        }

        public MapPoint(double latitude, double longitude)
        {
            Latitude = latitude;
            Longitude = longitude;
            Altitude =0;            
        }
        public MapPoint()
        { }

        public double XDistanceMeters(MapPoint Source)
        {
            if (Source == null)
                throw new NullReferenceException();

            return (Longitude - Source.Longitude) * 111000 * Math.Sin(Math.Abs(Source.Latitude));
        }
        public double YDistanceMeters(MapPoint Source)
        {
            if (Source==null)
                throw new NullReferenceException();

            return -(Latitude - Source.Latitude) * 111000;
        }
        public double SurfaceDistanceMeters(MapPoint Source)
        {
            if (Source == null)
                throw new NullReferenceException();

            //haversine formula
            double R = 6378137.0; // m    

            double dLat = (Math.PI / 180.0d) * (Latitude - Source.Latitude);
            double dLon = (Math.PI / 180.0d) * (Longitude - Source.Longitude);

            double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
                    Math.Cos(Source.Latitude * Math.PI / 180.0d) *
                    Math.Cos(Source.Latitude * Math.PI / 180.0d) *
                    Math.Sin(dLon / 2) * Math.Sin(dLon / 2);

            double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));

            return R * c;
        }

        public float BearingTo(MapPoint Dest)
        {
            double long1 = Longitude * (Math.PI / 180.0d);
            double long2 = Dest.Longitude * (Math.PI / 180.0d);
            double lat1 = Latitude * (Math.PI / 180.0d);
            double lat2 = Dest.Latitude * (Math.PI / 180.0d);

            double y = Math.Sin(long2-long1) * Math.Cos(lat2);
            double x = Math.Cos(lat1) * Math.Sin(lat2) -
                    Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(long2-long1);
            double brng = (double)Math.Atan2(y, x);

            brng = (((brng * 180.0d) / Math.PI) + 360.0d);  
            brng = brng%360;

            return (float)brng;
        }
    }
    
}
